/**
 * @file MotorDriver.cpp
 * @author SoBigDog (623644973lubin@163.com)
 * @brief 
 * @version 0.1
 * @date 2020-09-27
 * 
 * @copyright Copyright (c) 2020
 * 
 */
//#include "ProtocolDriver.h"
//#include "MotorDriver.h"
#include "NewProtocolCurtis.h"

namespace{
	const int STOP_CYCLE_LIMIT = 20;
	const int ENCODER_MAX_DIFF = 2000000;
}

CMotorDriverBase::CMotorDriverBase() :
	_driverProtocol(NULL),
	_encoder_timestamp_nsec_(0),
	_cur_ret_msg_num(0),
	_total_ret_msg_num(0)
{
//	if(_rxMailbox[0] == NULL)
//		_rxMailbox[0] = new CCanRxMailbox(msg_data[0], RX_MAILBOX_QUE_SIZE);
	_motorType = MOTOR_TYPE_NULL;
	_odo_time_window_counter = 0;
	_is_stopped = true;
	_is_moving = false;
	_inverse = 1;
	_scale = 1;
	_last_cnt = 0;
	_first_in = true;
	_pre_timestamp_nsec_ = 0;
	_isConnected = false;
	_stop_cycle = 0;
	_critical_delta = 1.1;
	_critical_window_delta = 5.5;
	_is_updated = false;
	_is_wait_process_start = false;
	_is_wait_process_connect = false;
	_is_NMT_process_connect = false;
}

bool CMotorDriverBase::read()
{
	return false;
}

int CMotorDriverBase::init()
{	
	if(!_isConnected || _is_wait_process_connect || _is_NMT_process_connect){
		_is_wait_process_connect = false;
		_is_NMT_process_connect = false;
	// TODO1 according to Message_ChassisConfig to set _rxMailbox id and attach _canBaseRouter
	//	_rxMailbox[0]->clear();
	//	_rxMailbox[1]->clear();
		_cur_ret_msg_num = 0;
		if(_cur_ret_msg_num < _total_ret_msg_num){
			//_canBaseRouter->putMsg(_ret_msg[_cur_ret_msg_num]);
			_cur_ret_msg_num++;
		} else {
			resetAfterConnected();
		}
	}

	return 0;
}

void CMotorDriverBase::sendCtrlVel(float s){
	_driverProtocol->encode(DRV_CMD_TARGET_SPEED, (int32_t)s);

}		

void CMotorDriverBase::sendTargetPos(int32_t s)
{
	_driverProtocol->encode(DRV_CMD_TARGET_POS, s);
}

void CMotorDriverBase::checkConnect(){
	_is_wait_process_start = true;
	_is_wait_process_connect = false;
	_driverProtocol->encode(DRV_CMD_ACTURAL_POS, 0);
}

void CMotorDriverBase::driverLifeGuard()
{
//	CanTxMsg TxMessage;
//	if (_driverProtocol->lifeguard(TxMessage)) {
//		_canBaseRouter->putMsg(TxMessage);
//	}
}
